/*
 * Copyright (c) 2014 The Plasix Authors
 * Plasix (Px) Library
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#ifndef PX_LINE_SEGMENT_H
#define PX_LINE_SEGMENT_H

#include <pxVector3.h>

namespace Px {
namespace Geometry {

template <typename Real>
class LineSegment {
    using Vector3 = Math::Vector3<Real>;
    enum Vertex { A, B };

public:
    LineSegment();
    LineSegment(const Vector3& a, const Vector3& b);
    LineSegment(Real ax, Real ay, Real az, Real bx, Real by, Real bz);
    virtual ~LineSegment();

    bool setPoint(bool index, const Vector3& point);
    void setPointA(const Vector3& point);
    void setPointB(const Vector3& point);

    bool setVertex(bool index, const Vector3& point);
    void setVertexA(const Vector3& point);
    void setVertexB(const Vector3& point);

    void getPoint(bool index, Vector3& point);
    void getVertex(bool index, Vector3& point);

    Real length() const;
    Real distanceToSegment(const LineSegment<Real>& segment, Vector3& aClosest, Vector3& bClosest);
    Real distanceToSegment(const Vector3& b0, const Vector3& b1, Vector3& aClosest, Vector3& bClosest);
    bool parallelTo(const LineSegment<Real>& segment, Real epsilon = 1.0e-2);
    bool parallelTo(const Vector3& a, const Vector3& b, Real epsilon = 1.0e-2);
    bool contains(const Vector3& point, bool inclusive = true, Real epsilon = 1.0e-4);
    Vector3 midpoint() const;    

    Vector3 vector() const;
    Vector3 getPointA() const;
    Vector3 getPoitnB() const;
    Vector3 getVertexA() const;
    Vector3 getvertexB() const;

    Vector3& a();
    Vector3& b();

    static bool Contains(const Vector3& a, const Vector3& b, const Vector3& point, bool inclusive = true, Real epsilon = 1.0e-4);
    static bool Aligned(const Vector3& a, const Vector3& b, const Vector3& c, Real epsilon = 1.0e-4);
    static bool Parallel(const Vector3& a0, const Vector3& a1, const Vector3& b0, const Vector3& b1, Real epsilon = 1.0e-2);
    static Real SegmentToSegmentDistance(const Vector3& a0, const Vector3& a1, const Vector3& b0, const Vector3& b1, Vector3& aClosest, Vector3& bClosest);
    static Vector3 Midpoint(const Vector3& a, const Vector3& b);

    const static unsigned char LINE_SEGMENT_VERTEX_COUNT = 3;

protected:
    Vector3 seg[LINE_SEGMENT_VERTEX_COUNT];
};

template <typename Real>
LineSegment<Real>::LineSegment() {
    this->seg[A] = Vector3(static_cast<Real>(0), static_cast<Real>(0), static_cast<Real>(0));
    this->seg[B] = Vector3(static_cast<Real>(1), static_cast<Real>(0), static_cast<Real>(0));
}

template <typename Real>
LineSegment<Real>::LineSegment(const Vector3& a, const Vector3& b) {
    this->seg[A] = a;
    this->seg[B] = b;
}

template <typename Real>
LineSegment<Real>::LineSegment(Real ax, Real ay, Real az, Real bx, Real by, Real bz) {
    this->seg[A] = Vector3(ax, ay, az);
    this->seg[B] = Vector3(bx, by, bz);
}

template <typename Real>
LineSegment<Real>::~LineSegment() {}

template <typename Real>
bool LineSegment<Real>::setPoint(bool index, const Vector3& point) {
    if ( index ) this->seg[B] = point;
    else this->seg[A] = point;
}

template <typename Real>
void LineSegment<Real>::setPointA(const Vector3& point) {
    this->seg[A] = point;
}

template <typename Real>
void LineSegment<Real>::setPointB(const Vector3& point) {
    this->seg[B] = point;
}

template <typename Real>
bool LineSegment<Real>::setVertex(bool index, const Vector3& point) {
    if ( index ) this->seg[B] = point;
    else this->set[A] = point;
}

template <typename Real>
void LineSegment<Real>::setVertexA(const Vector3& point) {
    this->seg[A] = point;
}

template <typename Real>
void LineSegment<Real>::setVertexB(const Vector3& point) {
    this->set[B] = point;
}

template <typename Real>
void LineSegment<Real>::getPoint(bool index, Vector3& point) {
    if ( index ) return this->seg[B];
    else return this->seg[A];
}

template <typename Real>
void LineSegment<Real>::getVertex(bool index, Vector3& point) {
    if ( index ) return this->seg[B];
    else return this->seg[A];
}

template <typename Real>
Real LineSegment<Real>::length() const {
    return Vector3::Distance(this->seg[A], this->seg[B]);
}

template <typename Real>
Real LineSegment<Real>::distanceToSegment(const LineSegment<Real>& segment, Vector3& aClosest, Vector3& bClosest) {
    return LineSegment<Real>::SegmentToSegmentDistance(this->seg[A], this->seg[B], segment.seg[A], segment.seg[B], aClosest, bClosest);
}

template <typename Real>
Real LineSegment<Real>::distanceToSegment(const Vector3& b0, const Vector3& b1, Vector3& aClosest, Vector3& bClosest) {
    return LineSegment<Real>::SegmentToSegmentDistance(this->seg[A], this->seg[B], b0, b1, aClosest, bClosest);
}

template <typename Real>
bool LineSegment<Real>::parallelTo(const LineSegment<Real>& segment, Real epsilon) {
    return LineSegment<Real>::Parallel(this->seg[A], this->seg[B], segment.a(), segment.b(), epsilon);
}

template <typename Real>
bool LineSegment<Real>::contains(const Vector3& point, bool inclusive, Real epsilon) {
    return LineSegment<Real>::Contains(this->seg[A], this->seg[B], point, inclusive, epsilon);
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::midpoint() const {
    return LineSegment<Real>::Midpoint(this->seg[A], this->seg[B]);
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::vector() const {
    return Vector3(this->seg[A], this->seg[B]);
}

template <typename Real>
bool LineSegment<Real>::parallelTo(const Vector3& a, const Vector3& b, Real epsilon) {
    return LineSegment<Real>::Parallel(this->seg[A], this->seg[B], a, b, epsilon);
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::getPointA() const {
    return this->seg[A];
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::getPoitnB() const {
    return this->seg[B];
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::getVertexA() const {
    return this->seg[A];
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::getvertexB() const {
    return this->seg[B];
}

template <typename Real>
typename LineSegment<Real>::Vector3& LineSegment<Real>::a() {
    return this->seg[A];
}

template <typename Real>
typename LineSegment<Real>::Vector3& LineSegment<Real>::b() {
    return this->seg[B];
}

template <typename Real>
bool LineSegment<Real>::Aligned(const Vector3& a, const Vector3& b, const Vector3& c, Real epsilon) {
    if ( (b - a).isZero(epsilon) ) return true;
    if ( (c - a).isZero(epsilon) ) return true;

    Vector3 ab = (b - a).normalized();
    Vector3 ac = (c - a).normalized();
    
    if ( ab.cross(ac) < epsilon ) return true;
    return false;
}

template <typename Real>
bool LineSegment<Real>::Contains(const Vector3& a, const Vector3& b, const Vector3& point, bool inclusive, Real epsilon) {
    //--------------------------------------------------------------------------
    // If the point is not aligned with the end points of this segment then
    // it cannot reside within this segment.
    //--------------------------------------------------------------------------
    if ( LineSegment<Real>::Aligned(a, b, point, epsilon) == false ) return false;
    Vector3 ab = (b - a);
    Vector3 ac = (point - a);
    
    double Kac = ab.dot(ac);
    double Kab = ab.dot(ab);

    if ( Kac < 0.0 ) return false;
    if ( Kac > Kab ) return false;
    if ( (Kac > -epsilon && Kac < epsilon) && inclusive ) return true;
    if ( (Kac > (Kab - epsilon) && Kac < (Kab + epsilon)) && inclusive ) return true;
    if ( (0.0 < Kac) && (Kac < Kab) ) return true;
    return false;
}

template <typename Real>
bool LineSegment<Real>::Parallel(const Vector3& a0, const Vector3& a1, const Vector3& b0, const Vector3& b1, Real epsilon) {
    if ( a0 == a1 ) {
        std::cerr << "[Px:LineSegment:Parallel] Warning: Line segment (a0, a1) is degenerate." << std::endl;
        return false;
    }

    if ( b0 == b1 ) {
        std::cerr << "[Px:LineSegment:Parallel] Warning: Line segment (b0, b1) is degenerate." << std::endl;
        return false;
    }

    Vector3 aDir = (a1 - a0).normalized();
    Vector3 bDir = (b1 - b0).normalized();
    Real dot = aDir.dot(bDir);

    if ( dot > (-1 - epsilon) && dot < (-1 + epsilon) ) return true;
    if ( dot < (1 + epsilon) && dot > (1 - epsilon) ) return true;
    return false;
}

template <typename Real>
Real LineSegment<Real>::SegmentToSegmentDistance(const Vector3& a0, const Vector3& a1, const Vector3& b0, const Vector3& b1, Vector3& aClosest, Vector3& bClosest) {
    Vector3 u = a1 - a0;
    Vector3 v = b1 - b0;
    Vector3 w = a0 - b0;

    double a = u.dot(u);
    double b = u.dot(v);
    double c = v.dot(v);
    double d = u.dot(w);
    double e = v.dot(w);

    double D = a*c - b*b;
    double sc, sN, sD = D;
    double tc, tN, tD = D;

    if ( D < std::numeric_limits<double>::epsilon() ) {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else {
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if ( sN > sD ) {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if ( tN < 0.0 ) {
        tN = 0.0;

        if ( -d < 0.0 )
            sN = 0.0;
        else if ( -d > a )
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if ( tN > tD ) {
        tN = tD;

        if ( (-d + b) < 0.0 )
            sN = 0;
        else if ( (-d + b) > a )
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }

    sc = (abs(sN) < std::numeric_limits<double>::epsilon() ? 0.0 : sN / sD);
    tc = (abs(tN) < std::numeric_limits<double>::epsilon() ? 0.0 : tN / tD);

    Vector3  dP = w + (u * sc) - (v * tc);
    aClosest = a0 + (u * sc);
    bClosest = b0 + (v * tc);
    return std::sqrt(dP.dot(dP));
}

template <typename Real>
typename LineSegment<Real>::Vector3 LineSegment<Real>::Midpoint(const Vector3& a, const Vector3& b) {
    return Vector3((a.getX() + b.getX()) / static_cast<Real>(2.0), (a.getY() + b.getY()) / static_cast<Real>(2.0), (a.getZ() + b.getZ()) / static_cast<Real>(2.0));
}

typedef LineSegment<double> LineSegmentd;
typedef LineSegment<float> LineSegmentf;

}

}

#endif
